/*
 * @Author: LVGRAPE
 * @Date: 2023-12-13 11:26:01
 * @LastEditTime: 2024-03-04 21:00:22
 * @LastEditors: EltonLi lijiaming@uavi-tech.com
 * @Description:
 * @FilePath: \zino-fc-v4\ZINO\hardware\Icm426xx\platform\lcm42688P_read_polling.c
 * 要啥没啥，爱咋咋的
 */

#include "Icm426xxTransport.h"
#include "Icm426xxDriver_HL.h"
#include "rtthread.h"
#include <drv_spi.h>
#include "zino.h"

#define DBG_TAG "ICM6288P"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>


#define SERIF_TYPE ICM426XX_UI_SPI4
//NOTE - 测试板用的qmi的总线,正常板子用的lsm的总线
#define ICM42688P_BUS_NAME "spi_lsm"
#define ICM_I2C_ADDR 0x68
#define USE_CLK_IN 0
#define SENSOR_ODR 0

const float gyro_full_scale_factor[8]={
    16.4f,//+/-2000dps
    32.8f,//+/-1000dps
    65.5f,//+/-500dps
    131.f,//+/-250dps
    256.f,//+/-125dps
    524.3f,//+/-62.5dps
    1048.6f,//+/-31.25dps
    2097.2f,//+/-15.625dsp
};

const uint16_t accel_full_scale_factor[4]={
    2048,//+/-16g
    4096,//+/-8g
    8192,//+/-4g
    16384//+/-2g
};

static struct inv_icm426xx_serif icm42688p_dev;
/* Just a handy variable to handle the icm426xx object */
static struct inv_icm426xx icm_driver;
/* Just a handy variable to handle the spi object */
static struct rt_spi_device* icm6288p_spi = RT_NULL;
/* Just a handy variable to handle the i2c object */
static struct rt_i2c_bus_device* icm6288p_i2c = RT_NULL;
/* Just a handy variable to handle the accel full scale factor */
static uint16_t accel_fs_factor=2048;
/* Just a handy variable to handle the gyro full scale factor */
static float gyro_fs_factor=16.4f;

/**
 * @brief 读寄存器
 * 
 * @param serif 
 * @param reg 
 * @param rbuffer 
 * @param rlen 
 * @return int 
 */
int inv_io_hal_read_reg(struct inv_icm426xx_serif* serif, uint8_t reg, uint8_t* rbuffer,
    uint32_t rlen)
{
    switch (serif->serif_type) {
    case ICM426XX_AUX1_SPI3:
    case ICM426XX_AUX1_SPI4:
    case ICM426XX_AUX2_SPI3:
    case ICM426XX_UI_SPI4:
        RT_ASSERT(icm6288p_spi != RT_NULL);
        reg |= 0x80;
        return rt_spi_send_then_recv(icm6288p_spi, &reg, 1, rbuffer, rlen);
    case ICM426XX_UI_I2C:
    {
        RT_ASSERT(icm6288p_i2c != RT_NULL);
        struct rt_i2c_msg msg[2];
        msg[0].addr = ICM_I2C_ADDR;
        msg[0].buf = &reg;
        msg[0].len = 1;
        msg[0].flags = RT_I2C_WR;

        msg[1].addr = ICM_I2C_ADDR;
        msg[1].buf = rbuffer;
        msg[1].len = rlen;
        msg[1].flags = RT_I2C_RD | RT_I2C_NO_START;
        return rt_i2c_transfer(icm6288p_i2c, msg, 2);
    }
    default:
        return -1;
    }
}
/**
 * @brief 写寄存器
 * 
 * @param serif 
 * @param reg 
 * @param wbuffer 
 * @param wlen 
 * @return int 
 */
int inv_io_hal_write_reg(struct inv_icm426xx_serif* serif, uint8_t reg, const uint8_t* wbuffer,
    uint32_t wlen)
{
    int rc;

    switch (serif->serif_type) {
    case ICM426XX_AUX1_SPI3:
    case ICM426XX_AUX1_SPI4:
    case ICM426XX_AUX2_SPI3:
    case ICM426XX_UI_SPI4:
        RT_ASSERT(icm6288p_spi != RT_NULL);
        for (uint32_t i = 0; i < wlen; i++) {
            uint8_t regAddr = reg + i;
            rc = rt_spi_send_then_send(icm6288p_spi, &regAddr, 1, &wbuffer[i], 1);
            if (rc)
                return rc;
        }
        return 0;
    case ICM426XX_UI_I2C:
    {
        RT_ASSERT(icm6288p_i2c != RT_NULL);
        struct rt_i2c_msg msg[2];
        msg[0].addr = ICM_I2C_ADDR;
        msg[0].buf = &reg;
        msg[0].len = 1;
        msg[0].flags = RT_I2C_WR;

        msg[1].addr = ICM_I2C_ADDR;
        msg[1].buf = (uint8_t *)wbuffer;
        msg[1].len = wlen;
        msg[1].flags = RT_I2C_WR | RT_I2C_NO_START;
        return rt_i2c_transfer(icm6288p_i2c, msg, 2);
    }
    default:
        return -1;
    }
}
/**
 * @brief 初始化spi,i2c接口给icm42688用，i2c没有调试
 * 
 * @param icm_serif 
 * @return int 
 */
static int platform_init(struct inv_icm426xx_serif* icm_serif)
{
    icm6288p_spi = (struct rt_spi_device*)rt_device_find(ICM42688P_BUS_NAME);
    if (icm6288p_spi != RT_NULL)
    {
        icm6288p_spi->config.max_hz = 20000000;
        icm6288p_spi->config.data_width = 8;
        icm6288p_spi->config.mode = RT_SPI_MODE_3 | RT_SPI_MSB;
        LOG_D("%s spi init ok!", ICM42688P_BUS_NAME);
    }
    else
    {
        LOG_E("init failed! can't find %s spi!", ICM42688P_BUS_NAME);
        return -RT_ERROR;
    }

    /* Initialize serial inteface between MCU and Icm426xx */
    icm_serif->context = 0; /* no need */
    icm_serif->read_reg = inv_io_hal_read_reg;
    icm_serif->write_reg = inv_io_hal_write_reg;
    icm_serif->max_read = 1024 * 32; /* maximum number of bytes allowed per serial read */
    icm_serif->max_write = 1024 * 32; /* maximum number of bytes allowed per serial write */
    icm_serif->serif_type = SERIF_TYPE;

    return RT_EOK;
}
void inv_icm426xx_sleep_us(uint32_t us)
{
    if (us < 1000)
        rt_hw_us_delay(us);
    else
        rt_thread_mdelay(us / 1000);

}
/**
 * @brief 初始化：上电，验证id
 * 
 * @param icm_serif 
 * @return int 
 */
int SetupInvDevice(struct inv_icm426xx_serif* icm_serif)
{
    int     rc = 0;
    uint8_t who_am_i;

    /* Initialize device */
    LOG_I("Initialize Icm42688p");

    rc = inv_icm426xx_init(&icm_driver, icm_serif, NULL);
    if (rc != RT_EOK) {
        LOG_E("!!! ERROR : failed to initialize Icm426xx.");
        return rc;
    }

    /* Check WHOAMI */
    LOG_I("Check Icm426xx whoami value");

    rc = inv_icm426xx_get_who_am_i(&icm_driver, &who_am_i);
    if (rc != INV_ERROR_SUCCESS) {
        LOG_E("!!! ERROR : failed to read Icm426xx whoami value.");
        return rc;
    }

    if (who_am_i != ICM_WHOAMI) {
        LOG_E("!!! ERROR :  bad WHOAMI value. Got 0x%02x (expected: 0x%02x)",
            who_am_i, ICM_WHOAMI);
        return INV_ERROR;
    }

    return rc;
}
/**
 * @brief 配置量程，ODR, LFP
 * 
 * @return int 
 */
int ConfigureInvDevice()
{
    int     rc = 0;
    uint8_t data;
    /* Set Pulse to support high rates */
    rc |= inv_icm426xx_read_reg(&icm_driver, MPUREG_INT_CONFIG1, 1, &data);
    data |=
        ((uint8_t)ICM426XX_INT_TPULSE_DURATION_8_US) | ((uint8_t)ICM426XX_INT_TDEASSERT_DISABLED);
    rc |= inv_icm426xx_write_reg(&icm_driver, MPUREG_INT_CONFIG1, 1, &data);

    /* Disable fifo usage, data will be read from sensors registers*/
    rc |= inv_icm426xx_configure_fifo(&icm_driver, INV_ICM426XX_FIFO_DISABLED);
    if (rc != INV_ERROR_SUCCESS) {
        LOG_E("!!! ERROR : failed to initialize Icm426xx.");
        return rc;
    }

#if defined(ICM42633) && USE_CLK_IN
    /*
     * ICM42633 is a triple interface device. To access CLKIN, AUX2 interface needs to be disabled.
     * Use INV_ICM426XX_DUAL_INTERFACE mode. The following mode are also compatible:
     *  - INV_ICM426XX_SINGLE_INTERFACE
     *  - INV_ICM426XX_DUAL_INTERFACE_SPI4
     */
    rc |= inv_icm426xx_interface_change_procedure(&icm_driver, INV_ICM426XX_DUAL_INTERFACE);
#endif
    rc |= inv_icm426xx_enable_clkin_rtc(&icm_driver, USE_CLK_IN);

    rc |= inv_icm426xx_set_accel_fsr(&icm_driver, ICM426XX_ACCEL_CONFIG0_FS_SEL_8g);
    rc |= inv_icm426xx_set_gyro_fsr(&icm_driver, ICM426XX_GYRO_CONFIG0_FS_SEL_2000dps);

    rc |= inv_icm426xx_set_accel_frequency(&icm_driver, ICM426XX_ACCEL_CONFIG0_ODR_200_HZ);
    rc |= inv_icm426xx_set_gyro_frequency(&icm_driver, ICM426XX_GYRO_CONFIG0_ODR_200_HZ);
    // rc |= inv_icm426xx_set_accel_frequency(&icm_driver, ICM426XX_ACCEL_CONFIG0_ODR_1_KHZ);
    // rc |= inv_icm426xx_set_gyro_frequency(&icm_driver, ICM426XX_GYRO_CONFIG0_ODR_1_KHZ);

// #if SENSOR_ODR == HIGH_RATE_8KHZ
//     rc |= inv_icm426xx_set_accel_frequency(&icm_driver, ICM426XX_ACCEL_CONFIG0_ODR_8_KHZ);
//     rc |= inv_icm426xx_set_gyro_frequency(&icm_driver, ICM426XX_GYRO_CONFIG0_ODR_8_KHZ);
// #elif SENSOR_ODR == HIGH_RATE_16KHZ
//     rc |= inv_icm426xx_set_accel_frequency(&icm_driver, ICM426XX_ACCEL_CONFIG0_ODR_16_KHZ);
//     rc |= inv_icm426xx_set_gyro_frequency(&icm_driver, ICM426XX_GYRO_CONFIG0_ODR_16_KHZ);
// #elif SENSOR_ODR == HIGH_RATE_32KHZ
//     rc |= inv_icm426xx_set_accel_frequency(&icm_driver, ICM426XX_ACCEL_CONFIG0_ODR_32_KHZ);
//     rc |= inv_icm426xx_set_gyro_frequency(&icm_driver, ICM426XX_GYRO_CONFIG0_ODR_32_KHZ);
// #endif
    //NOTE 开启低噪声模式
    rc |= inv_icm426xx_enable_gyro_low_noise_mode(&icm_driver);
    rc |= inv_icm426xx_enable_accel_low_noise_mode(&icm_driver);




//rc |= inv_icm426xx_set_accel_ln_bw(&icm_driver, ICM426XX_GYRO_ACCEL_CONFIG0_GYRO_FILT_BW_20);
// rc |= inv_icm426xx_set_gyro_ln_bw(&icm_driver, ICM426XX_GYRO_ACCEL_CONFIG0_GYRO_FILT_BW_40);


    //NOTE 读数量程系数，原始数据除以这个数能得真实值
    ICM426XX_ACCEL_CONFIG0_FS_SEL_t accel_fsr;
    ICM426XX_GYRO_CONFIG0_FS_SEL_t gyro_fsr;
    rc |= inv_icm426xx_get_accel_fsr(&icm_driver, &accel_fsr);
    rc |= inv_icm426xx_get_gyro_fsr(&icm_driver, &gyro_fsr);
    accel_fs_factor = accel_full_scale_factor[(accel_fsr>>BIT_ACCEL_CONFIG0_FS_SEL_POS)%4];
    gyro_fs_factor = gyro_full_scale_factor[(gyro_fsr>>BIT_GYRO_CONFIG0_FS_SEL_POS)%8];
    LOG_D("accel_fs_factor:%d",accel_fs_factor);
    LOG_D("gyro_fs_factor:%d.%01d",(int)gyro_fs_factor,(int)(gyro_fs_factor*10)%10);

    /* Wait the maximum startup time in case gyro is enbaled */
    inv_icm426xx_sleep_us(ICM426XX_GYR_STARTUP_TIME_US);

    return rc;
}
uint64_t inv_icm426xx_get_time_us(void)
{
    return rt_tick_get_millisecond()*1000;
}
/**
 * @brief 配置icm42688,上电，量程，odr
 * 
 * @return int 
 */
int icm42688p_read_polling_init(void)
{
    rt_thread_mdelay(25);//NOTE waite for power stable
    if (platform_init(&icm42688p_dev) != RT_EOK)
    {
        LOG_E("icm42688p_read_polling_init failed!");
        return -RT_ERROR;
    }
    LOG_I("platform_init ok!");
    if (SetupInvDevice(&icm42688p_dev) != RT_EOK)
    {
        LOG_E("SetupInvDevice error!");
        return -RT_ERROR;
    }
    else
    {
        LOG_I("SetupInvDevice ok!");
    }
    
    if(ConfigureInvDevice() != RT_EOK)
    {
        LOG_E("ConfigureInvDevice error!");
        return -RT_ERROR;
    }
    else
    {
        LOG_I("ConfigureInvDevice ok!");
    }
    // int16_t readbuffer[6];
    // for(uint16_t i=0; i<1000;i++)
    // {
    //     inv_io_hal_read_reg(&icm42688p_dev, MPUREG_ACCEL_DATA_X0_UI, (uint8_t *)readbuffer, 12);
    //     rt_kprintf("acc: %x \t %x \t %x \t gyro: %x \t %x \t %x \n",
    //     readbuffer[0],readbuffer[1],readbuffer[2],readbuffer[3],readbuffer[4],readbuffer[5]);

    //     rt_thread_mdelay(5);
    // }
    return RT_EOK;
}
/**
 * @brief 
 * 
 * @param ax mg
 * @param ay mg
 * @param az mg
 * @return int 
 */
int icm42688p_accel_read_polling(int32_t *ax, int32_t *ay, int32_t *az)
{
    uint8_t accelBuffer[6];
    int ret =0;
    ret = inv_io_hal_read_reg(&icm42688p_dev, MPUREG_ACCEL_DATA_X0_UI, (uint8_t *)accelBuffer, 6);
    int16_t raw_ax = (int16_t)(accelBuffer[0] << 8 | accelBuffer[1]);
    int16_t raw_ay = (int16_t)(accelBuffer[2] << 8 | accelBuffer[3]);    
    int16_t raw_az = (int16_t)(accelBuffer[4] << 8 | accelBuffer[5]);
    /**
     * @brief NOTE AXIS:
     *         X^
     *          |
     *  Y <-----|
     */
    *ay = -(long)raw_ax*1000/accel_fs_factor;
    *ax = -(long)raw_ay*1000/accel_fs_factor;
    *az = -(long)raw_az*1000/accel_fs_factor;
    // rt_kprintf("raw acc: %d \t %d \t %d\n, ",raw_ax,raw_ay,raw_az);
    return ret;
}
/**
 * @brief 
 * 
 * @param gx mDps*1000
 * @param gy mDps*1000
 * @param gz mDps*1000
 * @return int 
 */
int icm42688p_gyro_read_polling(int32_t *gx, int32_t *gy, int32_t *gz)
{
    uint8_t gyroBuffer[6];
    int ret =0;
    ret = inv_io_hal_read_reg(&icm42688p_dev, MPUREG_GYRO_DATA_X0_UI, (uint8_t *)gyroBuffer, 6);
    int16_t raw_gx = (int16_t)(gyroBuffer[0] << 8 | gyroBuffer[1]);
    int16_t raw_gy = (int16_t)(gyroBuffer[2] << 8 | gyroBuffer[3]);    
    int16_t raw_gz = (int16_t)(gyroBuffer[4] << 8 | gyroBuffer[5]);
    /**
     * @brief NOTE AXIS:
     *         X^
     *          |
     *  Y <-----|
     */
    *gy = -(float)raw_gx*1000000.f/gyro_fs_factor;
    *gx = -(float)raw_gy*1000000.f/gyro_fs_factor;
    *gz = -(float)raw_gz*1000000.f/gyro_fs_factor;
    // rt_kprintf("raw gyro: %d \t %d \t %d\n, ",raw_gx,raw_gy,raw_gz);
    return ret;
}

int icm42688p_gyroraw_read_polling(int16_t *gx, int16_t *gy, int16_t *gz)
{
    int ret =0;
    uint8_t gyroBuffer[6];
    ret = inv_io_hal_read_reg(&icm42688p_dev, MPUREG_GYRO_DATA_X0_UI, (uint8_t *)gyroBuffer, 6);
    *gx = (int16_t)(gyroBuffer[0] << 8 | gyroBuffer[1]);
    *gy = (int16_t)(gyroBuffer[2] << 8 | gyroBuffer[3]);  
    *gz = (int16_t)(gyroBuffer[4] << 8 | gyroBuffer[5]);
    return ret;
}

int icm42688p_accraw_read_polling(int16_t *ax, int16_t *ay, int16_t *az)
{
    int ret =0;
    uint8_t accBuffer[6];
    ret = inv_io_hal_read_reg(&icm42688p_dev, MPUREG_ACCEL_DATA_X0_UI, (uint8_t *)accBuffer, 6);
    *ax = (int16_t)(accBuffer[0] << 8 | accBuffer[1]);
    *ay = (int16_t)(accBuffer[2] << 8 | accBuffer[3]);
    *az = (int16_t)(accBuffer[4] << 8 | accBuffer[5]);
    return ret;
}

// ZINO_APP_EXPORT(icm42688p_read_polling_init);